////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WNMemory/inc/WNMemory.h"
#include "WNCore/inc/WNAssert.h"
#include "WNMath/inc/WNMatrix33.h"
#include "WNMath/inc/WNMatrix44.h"
#include "WNMath/inc/WNVector2.h"
#include "WNMath/inc/WNVector3.h"

namespace WNMath {
    WNMatrix33 operator * (WN_FLOAT32 _f, const WNMatrix33& _matrix) {
        return(WNMatrix33(_f * _matrix._11, _f * _matrix._12, _f * _matrix._13,
                          _f * _matrix._21, _f * _matrix._22, _f * _matrix._23,
                          _f * _matrix._31, _f * _matrix._32, _f * _matrix._33));
    }

    WNMatrix33 operator / (WN_FLOAT32 _f, const WNMatrix33& _matrix) {
        const WN_FLOAT32 fInv = 1.0f / _f;

        return(WNMatrix33(fInv * _matrix._11, fInv * _matrix._12, fInv * _matrix._13,
                          fInv * _matrix._21, fInv * _matrix._22, fInv * _matrix._23,
                          fInv * _matrix._31, fInv * _matrix._32, fInv * _matrix._33));
    }
}

using namespace WNMath;
using namespace WNMemory;

// Constructors ////////////////////////////////////////////////////////////////////////////////////

WNMatrix33::WNMatrix33() :
    _11(1.0f), _12(0.0f), _13(0.0f),
    _21(0.0f), _22(1.0f), _23(0.0f),
    _31(0.0f), _32(0.0f), _33(1.0f) {
}

WNMatrix33::WNMatrix33(const WN_FLOAT32* _f) :
    _11(_f[0]), _12(_f[1]), _13(_f[2]),
    _21(_f[3]), _22(_f[4]), _23(_f[5]),
    _31(_f[6]), _32(_f[7]), _33(_f[8]) {
}

WNMatrix33::WNMatrix33(const WNVector2& _vector1,
                       const WNVector2& _vector2,
                       const WNVector2& _vector3) :
    _11(_vector1.x), _12(_vector2.x), _13(_vector3.x),
    _21(_vector1.y), _22(_vector2.y), _23(_vector3.y),
          _31(0.0f),       _32(0.0f),       _33(1.0f) {
}

WNMatrix33::WNMatrix33(WN_FLOAT32 _m11, WN_FLOAT32 _m12, WN_FLOAT32 _m13,
                       WN_FLOAT32 _m21, WN_FLOAT32 _m22, WN_FLOAT32 _m23,
                       WN_FLOAT32 _m31, WN_FLOAT32 _m32, WN_FLOAT32 _m33) :
    _11(_m11), _12(_m12), _13(_m13),
    _21(_m21), _22(_m22), _23(_m23),
    _31(_m31), _32(_m32), _33(_m33) {
}

// Operators ///////////////////////////////////////////////////////////////////////////////////////

WN_FLOAT32& WNMatrix33::operator () (WN_INT32 _nRow, WN_INT32 _nCol) {
    WN_DEBUG_ASSERT(_nRow < 3 && _nCol < 3);

    return(*(&_11 + (((_nRow - 1) * 3) + _nCol)));
}

WN_FLOAT32 WNMatrix33::operator () (WN_INT32 _nRow, WN_INT32 _nCol) const {
    WN_DEBUG_ASSERT(_nRow < 3 && _nCol < 3);

    return(*(&_11 + (((_nRow - 1) * 3) + _nCol)));
}

WNMatrix33::operator WN_FLOAT32* () {
    return(&_11);
}

WNMatrix33::operator const WN_FLOAT32* () const {
    return(&_11);
}

WNMatrix33& WNMatrix33::operator *= (const WNMatrix33& _matrix) {
    const WNMatrix33 mat = *this;

    _11 = mat._11 * _matrix._11 + mat._12 * _matrix._21 + mat._13 * _matrix._31;
    _12 = mat._11 * _matrix._12 + mat._12 * _matrix._22 + mat._13 * _matrix._32;
    _13 = mat._11 * _matrix._13 + mat._12 * _matrix._23 + mat._13 * _matrix._33;
    _21 = mat._21 * _matrix._11 + mat._22 * _matrix._21 + mat._23 * _matrix._31;
    _22 = mat._21 * _matrix._12 + mat._22 * _matrix._22 + mat._23 * _matrix._32;
    _23 = mat._21 * _matrix._13 + mat._22 * _matrix._23 + mat._23 * _matrix._33;
    _31 = mat._31 * _matrix._11 + mat._32 * _matrix._21 + mat._33 * _matrix._31;
    _32 = mat._31 * _matrix._12 + mat._32 * _matrix._22 + mat._33 * _matrix._32;
    _33 = mat._31 * _matrix._13 + mat._32 * _matrix._23 + mat._33 * _matrix._33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator += (const WNMatrix33& _matrix) {
    _11 += _matrix._11; _12 += _matrix._12; _13 += _matrix._13;
    _21 += _matrix._21; _22 += _matrix._22; _23 += _matrix._23;
    _31 += _matrix._31; _32 += _matrix._32; _33 += _matrix._33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator -= (const WNMatrix33& _matrix) {
    _11 -= _matrix._11; _12 -= _matrix._12; _13 -= _matrix._13;
    _21 -= _matrix._21; _22 -= _matrix._22; _23 -= _matrix._23;
    _31 -= _matrix._31; _32 -= _matrix._32; _33 -= _matrix._33;

    return(*this);
}

WNMatrix33& WNMatrix33::operator *= (WN_FLOAT32 _f) {
    _11 *= _f; _12 *= _f; _13 *= _f;
    _21 *= _f; _22 *= _f; _23 *= _f;
    _31 *= _f; _32 *= _f; _33 *= _f;

    return(*this);
}

WNMatrix33& WNMatrix33::operator /= (WN_FLOAT32 _f) {
    const WN_FLOAT32 fInv = 1.0f / _f;

    _11 *= fInv; _12 *= fInv; _13 *= fInv;
    _21 *= fInv; _22 *= fInv; _23 *= fInv;
    _31 *= fInv; _32 *= fInv; _33 *= fInv;

    return(*this);
}

WNMatrix33 WNMatrix33::operator + () const {
    return(*this);
}

WNMatrix33 WNMatrix33::operator - () const {
    return(WNMatrix33(-_11, -_12, -_13,
                      -_21, -_22, -_23,
                      -_31, -_32, -_33));
}

WNMatrix33 WNMatrix33::operator * (const WNMatrix33& _matrix) const {
    const WNMatrix33 mat1 = *this;
    const WNMatrix33 mat2 = _matrix;

    WNMatrix33 mat;

    mat._11 = mat1._11 * mat2._11 + mat1._12 * mat2._21 + mat1._13 * mat2._31;
    mat._12 = mat1._11 * mat2._12 + mat1._12 * mat2._22 + mat1._13 * mat2._32;
    mat._13 = mat1._11 * mat2._13 + mat1._12 * mat2._23 + mat1._13 * mat2._33;
    mat._21 = mat1._21 * mat2._11 + mat1._22 * mat2._21 + mat1._23 * mat2._31;
    mat._22 = mat1._21 * mat2._12 + mat1._22 * mat2._22 + mat1._23 * mat2._32;
    mat._23 = mat1._21 * mat2._13 + mat1._22 * mat2._23 + mat1._23 * mat2._33;
    mat._31 = mat1._31 * mat2._11 + mat1._32 * mat2._21 + mat1._33 * mat2._31;
    mat._32 = mat1._31 * mat2._12 + mat1._32 * mat2._22 + mat1._33 * mat2._32;
    mat._33 = mat1._31 * mat2._13 + mat1._32 * mat2._23 + mat1._33 * mat2._33;

    return(mat);
}

WNMatrix33 WNMatrix33::operator + (const WNMatrix33& _matrix) const {
    return(WNMatrix33(_11 + _matrix._11, _12 + _matrix._12, _13 + _matrix._13,
                      _21 + _matrix._21, _22 + _matrix._22, _23 + _matrix._23,
                      _31 + _matrix._31, _32 + _matrix._32, _33 + _matrix._33));
}

WNMatrix33 WNMatrix33::operator - (const WNMatrix33& _matrix) const {
    return(WNMatrix33(_11 - _matrix._11, _12 - _matrix._12, _13 - _matrix._13,
                      _21 - _matrix._21, _22 - _matrix._22, _23 - _matrix._23,
                      _31 - _matrix._31, _32 - _matrix._32, _33 - _matrix._33));
}

WNMatrix33 WNMatrix33::operator * (WN_FLOAT32 _f) const {
    return(WNMatrix33(_11 * _f, _12 * _f, _13 * _f,
                      _21 * _f, _22 * _f, _23 * _f,
                      _31 * _f, _32 * _f, _33 * _f));
}

WNMatrix33 WNMatrix33::operator / (WN_FLOAT32 _f) const {
    const WN_FLOAT32 fInv = 1.0f / _f;

    return(WNMatrix33(_11 * fInv, _12 * fInv, _13 * fInv,
                      _21 * fInv, _22 * fInv, _23 * fInv,
                      _31 * fInv, _32 * fInv, _33 * fInv));
}

WN_BOOL WNMatrix33::operator == (const WNMatrix33& _matrix) const {
    return(0 == WNMemCmp(this, &_matrix, sizeof(WNMatrix33)));
}

WN_BOOL WNMatrix33::operator != (const WNMatrix33& _matrix) const {
    return(0 != WNMemCmp(this, &_matrix, sizeof(WNMatrix33)));
}

// Members /////////////////////////////////////////////////////////////////////////////////////////

WN_VOID WNMatrix33::Zero() {
    WNMemSet(&_11, 0, sizeof(WNMatrix33));
}

WN_VOID WNMatrix33::Set(const WN_FLOAT32* _f) {
    WNMemCpy(&_11, _f, sizeof(WNMatrix33));
}

WN_VOID WNMatrix33::Set(WN_FLOAT32 _m11, WN_FLOAT32 _m12, WN_FLOAT32 _m13,
                        WN_FLOAT32 _m21, WN_FLOAT32 _m22, WN_FLOAT32 _m23,
                        WN_FLOAT32 _m31, WN_FLOAT32 _m32, WN_FLOAT32 _m33) {
    _11 = _m11; _12 = _m12; _13 = _m13;
    _21 = _m21; _22 = _m22; _23 = _m23;
    _31 = _m31; _32 = _m32; _33 = _m33;
}

WN_VOID WNMatrix33::Identity() {
    _11 = 1.0f; _12 = 0.0f; _13 = 0.0f;
    _21 = 0.0f; _22 = 1.0f; _23 = 0.0f;
    _31 = 0.0f; _32 = 0.0f; _33 = 1.0f;
}

WN_BOOL WNMatrix33::IsIdentity() const {
    return(_11 == 1.0f && _12 == 0.0f && _13 == 0.0f &&
           _21 == 0.0f && _22 == 1.0f && _23 == 0.0f &&
           _31 == 0.0f && _32 == 0.0f && _33 == 1.0f);
}

WN_BOOL WNMatrix33::IsSymmetric() const {
    if ((_12 - _21) != 0.0f) {
        return(WN_FALSE);
    }

    if ((_13 - _31) != 0.0f) {
        return(WN_FALSE);
    }

    if ((_23 - _32) != 0.0f) {
        return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_BOOL WNMatrix33::IsDiagonal() const {
    if (_12 != 0.0f || _13 != 0.0f ||
        _21 != 0.0f || _23 != 0.0f ||
        _31 != 0.0f || _32 != 0.0f) {
        return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_BOOL WNMatrix33::IsRotated() const {
    if (_12 == 0.0f && _21 == 0.0f) {
            return(WN_FALSE);
    }

    return(WN_TRUE);
}

WN_FLOAT32 WNMatrix33::Determinant() const {
    const WNMatrix33 mat = *this;

    return((mat._11 * mat._22 * mat._33 + mat._12 * mat._23 * mat._31 + mat._13 * mat._21 * mat._32) -
           (mat._31 * mat._22 * mat._13 + mat._32 * mat._23 * mat._11 + mat._33 * mat._21 * mat._12));
}

WN_FLOAT32 WNMatrix33::Trace() const {
    const WNMatrix33 mat = *this;

    return(mat._11 + mat._22 + mat._33);
}

WN_VOID WNMatrix33::Transpose() {
    const WNMatrix33 mat = *this;

    _11 = mat._11;
    _12 = mat._21;
    _13 = mat._31;
    _21 = mat._12;
    _22 = mat._22;
    _23 = mat._32;
    _31 = mat._13;
    _32 = mat._23;
    _33 = mat._33;
}

WN_BOOL WNMatrix33::Invert() {
    const WN_FLOAT32 determinant = Determinant();

    if (determinant == 0.0f) {
        return(WN_FALSE);
    }

    const WN_FLOAT32 invDet = 1.0f / determinant;
    const WNMatrix33 adjoint = GetAdjugate();

    *this = adjoint * invDet;

    return(WN_TRUE);
}

WN_VOID WNMatrix33::SetRow(WN_UINT8 _row, const WNVector3& _data) {
    WN_DEBUG_ASSERT(_row > 2);

    const WN_UINT8 offset = _row * 3;

    *(&_11 + offset) = _data.x;
    *(&_11 + offset + 1) = _data.y;
    *(&_11 + offset + 2) = _data.z;
}

WN_VOID WNMatrix33::SetColumn(WN_UINT8 _column, const WNVector3& _data) {
    WN_DEBUG_ASSERT(_column > 2);

    *(&_11 + _column) = _data.x;
    *(&_11 + _column + 3) = _data.y;
    *(&_11 + _column + 6) = _data.z;
}

WNMatrix33 WNMatrix33::GetTransposed() const {
    WNMatrix33 mat = *this;

    return(mat.Transpose(), mat);
}

WNMatrix33 WNMatrix33::GetInverted() const {
    WNMatrix33 mat = *this;

    return(mat.Invert(), mat);
}

WNMatrix33 WNMatrix33::GetAdjugate() const {
    WNMatrix33 matrix = *this;

    const WN_FLOAT32 mat11 = matrix._22 * matrix._33 - matrix._23 * matrix._32;
    const WN_FLOAT32 mat12 = -(matrix._12 * matrix._33 - matrix._13 * matrix._32);
    const WN_FLOAT32 mat13 = matrix._12 * matrix._23 - matrix._13 * matrix._22;
    const WN_FLOAT32 mat21 = -(matrix._21 * matrix._33 - matrix._23 * matrix._31);
    const WN_FLOAT32 mat22 = matrix._11 * matrix._33 - matrix._13 * matrix._31;
    const WN_FLOAT32 mat23 = -(matrix._11 * matrix._23 - matrix._13 * matrix._21);
    const WN_FLOAT32 mat31 = matrix._21 * matrix._32 - matrix._22 * matrix._31;
    const WN_FLOAT32 mat32 = -(matrix._11 * matrix._32 - matrix._12 * matrix._31);
    const WN_FLOAT32 mat33 = matrix._11 * matrix._22 - matrix._12 * matrix._21;

    matrix._11 = mat11;
    matrix._12 = mat12;
    matrix._13 = mat13;
    matrix._21 = mat21;
    matrix._22 = mat22;
    matrix._23 = mat23;
    matrix._31 = mat31;
    matrix._32 = mat32;
    matrix._33 = mat33;

    return(matrix);
}

WNVector3 WNMatrix33::GetRow(WN_UINT8 _row) const {
    WN_DEBUG_ASSERT(_row > 2);

    const WN_UINT8 offset = _row * 3;

    return(WNVector3(*(&_11 + offset),
                       *(&_11 + offset + 1),
                       *(&_11 + offset + 2)));
}

WNVector3 WNMatrix33::GetColumn(WN_UINT8 _column) const {
    WN_DEBUG_ASSERT(_column > 2);

    return(WNVector3(*(&_11 + _column),
                     *(&_11 + _column + 3),
                     *(&_11 + _column + 6)));
}